#include "MotionController_10.h"

extern uint8_t rx_buffer[];
extern volatile uint8_t recv_end_flag;

static char _cachedPosition[M114_BUFFER_SIZE];

// 初始化电机映射
void MotionController::initMotorMap()
{
    motorMap['A'] = &_aMotor;
    motorMap['B'] = &_bMotor;
    motorMap['C'] = &_cMotor;
    motorMap['D'] = &_dMotor;
    motorMap['E'] = &_eMotor;
    motorMap['L'] = &_lMotor;
    motorMap['M'] = &_mMotor;
    motorMap['N'] = &_nMotor;
    motorMap['O'] = &_oMotor;
    motorMap['P'] = &_pMotor;
}

MotionController::MotionController()
    : _homingState(NOT_HOMING),                                 // 当前归零状态 ======== 各轴分开了 好像没有用到??
      _homingStartTime(0),                                      // 归零开始时间
      _aMotor(MOTOR_A, A_DIRECTION, STEPS_PER_MM_A, A_MAX_POS), // A轴步进电机对象
      _bMotor(MOTOR_B, B_DIRECTION, STEPS_PER_MM_B, B_MAX_POS), // B轴步进电机对象
      _cMotor(MOTOR_C, C_DIRECTION, STEPS_PER_MM_C, C_MAX_POS), // C轴步进电机对象
      _dMotor(MOTOR_D, D_DIRECTION, STEPS_PER_MM_D, D_MAX_POS), // D轴步进电机对象
      _eMotor(MOTOR_E, E_DIRECTION, STEPS_PER_MM_E, E_MAX_POS), // E轴步进电机对象
      _lMotor(MOTOR_L, L_DIRECTION, STEPS_PER_MM_L, L_MAX_POS), // L轴步进电机对象
      _mMotor(MOTOR_M, M_DIRECTION, STEPS_PER_MM_M, M_MAX_POS), // M轴步进电机对象
      _nMotor(MOTOR_N, N_DIRECTION, STEPS_PER_MM_N, N_MAX_POS), // N轴步进电机对象
      _oMotor(MOTOR_O, O_DIRECTION, STEPS_PER_MM_O, O_MAX_POS), // O轴步进电机对象
      _pMotor(MOTOR_P, P_DIRECTION, STEPS_PER_MM_P, P_MAX_POS), // P轴步进电机对象
      _globalFeedrate(MAX_FEEDRATE_A),                          // 使用A轴作为默认值 ====新加
      _absoluteMode(true),                                      // 坐标模式（true=绝对坐标）
      _lastPositionUpdate(0),                                   // 上次位置更新时间初始化为0
      _isHoming(false),                                         // 归零状态标志
      _isEmergencyStop(false),                                  // 紧急停止标志
      _currentAcceleration(MAX_ACCELERATION_A)                  // 设置默认加速度
{
    // 初始化各轴速度
    _aFeedrate = MAX_FEEDRATE_A; // A轴最大进给速度
    _bFeedrate = MAX_FEEDRATE_B; // B轴最大进给速度
    _cFeedrate = MAX_FEEDRATE_C; // C轴最大进给速度
    _dFeedrate = MAX_FEEDRATE_D; // D轴最大进给速度
    _eFeedrate = MAX_FEEDRATE_E; // E轴最大进给速度
    _lFeedrate = MAX_FEEDRATE_L; // L轴最大进给速度
    _mFeedrate = MAX_FEEDRATE_M; // M轴最大进给速度
    _nFeedrate = MAX_FEEDRATE_N; // N轴最大进给速度
    _oFeedrate = MAX_FEEDRATE_O; // O轴最大进给速度
    _pFeedrate = MAX_FEEDRATE_P; // P轴最大进给速度

    // 初始化归零请求标志（将所有轴的归零请求设置为false）
    for (char axis : axisLetters)
    {
        _homeRequested[axis] = false;
    }

    // 初始化电机映射（建立轴标识与电机对象的关联）
    initMotorMap();
}

// 系统初始化函数
void MotionController::init()
{
    _initStartTime = HAL_GetTick();
    _initialized = false;

    // 初始化所有电机
    for (char axis : axisLetters)
    {
        motorMap[axis]->init();
    }

    // 设置默认速度和加速度
    setFeedrate(MAX_FEEDRATE_A);         // 设置默认进给速度
    setAcceleration(MAX_ACCELERATION_A); // 设置默认加速度

    // 初始化位置缓存
    memset(_cachedPosition, 0, M114_BUFFER_SIZE); // 新加
    _cachedPosition[0] = '\0';                    // 确保初始为空字符串

    printf("System initializing...\n");
}

// 上电自检函数 ======== 新加
void MotionController::powerOnSelfTest()
{
    printf("=== Power-On Self Test ===\n");

    auto getLimitSwitch = [](char axis) -> std::pair<GPIO_TypeDef *, uint16_t>
    {
        switch (axis)
        {
        case 'A':
            return {LIM1_GPIO_Port, LIM1_Pin};
        case 'B':
            return {LIM2_GPIO_Port, LIM2_Pin};
        case 'C':
            return {LIM3_GPIO_Port, LIM3_Pin};
        case 'D':
            return {LIM4_GPIO_Port, LIM4_Pin};
        case 'E':
            return {LIM5_GPIO_Port, LIM5_Pin};
        case 'L':
            return {LIM6_GPIO_Port, LIM6_Pin};
        case 'M':
            return {LIM7_GPIO_Port, LIM7_Pin};
        case 'N':
            return {LIM8_GPIO_Port, LIM8_Pin};
        case 'O':
            return {LIM9_GPIO_Port, LIM9_Pin};
        case 'P':
            return {LIM10_GPIO_Port, LIM10_Pin};
        default:
            return {nullptr, 0};
        }
    };

    // 1. 检查限位开关状态
    bool limit_triggered = false; // 是否有限位开关被触发
    for (char axis : axisLetters)
    {
        auto limitPin = getLimitSwitch(axis);
        if (limitPin.first == nullptr)
            continue;

        bool triggered = (HAL_GPIO_ReadPin(limitPin.first, limitPin.second) == GPIO_PIN_RESET); // 最小限位开关低电平触发

        if (triggered)
        {
            printf("Axis %c limit triggered at power on\n", axis);
            limit_triggered = true;
        }
    }

    if (!limit_triggered)
    {
        printf("All limits Not triggered\n");
    }

    // 2. 检查电机状态
    // ... 其他自检项目 ...

    printf("=== Self Test Complete ===\n");
}

// 更新函数 - 处理状态机、位置报告等
void MotionController::update()
{
    // 1. 紧急停止检查
    if (_isEmergencyStop)
        return;

    // 2. 处理串口命令
    if (recv_end_flag)
    {
        // printf("接收到串口输入,开始processCommand\r\n");
        processCommand((const char *)rx_buffer);
        recv_end_flag = 0; // 清除标志
    }

    // 3. 检查限位开关
    checkEndstops();

    // // 4. 更新所有电机状态
    // for (char axis : axisLetters)
    // {
    //     motorMap[axis]->update();
    // }

    // =============== 4. new ===============
    // 第一步：更新所有电机状态，判断是否需要步进（仅标记，无IO）
    for (char axis : axisLetters)
    {
        motorMap[axis]->update();
    }

    // 第二步：收集需要步进的电机
    std::vector<StepperMotor *> motorsToStep;
    for (char axis : axisLetters)
    {
        StepperMotor *motor = motorMap[axis];
        if (motor->isMoving() && motor->isEnabled() && motor->needStep())
        {
            motorsToStep.push_back(motor);
        }
    }

    // 第三步：并行生成所有步进信号，统一延时
    if (!motorsToStep.empty())
    {
        // 3.1 同时设置所有电机的STEP信号为高电平
        for (StepperMotor *motor : motorsToStep)
        {
            motor->generateStepSignal();
        }
        motor_update_outputs(); // 一次性同步高电平 

        // 3.2 统一延时（替代每个电机的独立延时）
        delay_us(1);

        // 3.3 同时设置所有电机的STEP信号为低电平
        for (StepperMotor *motor : motorsToStep)
        {
            motor->generateStepSignal(); // 翻转信号为低电平
        }
        motor_update_outputs(); // 一次性同步低电平 

        // 3.4 批量更新所有电机的位置（无IO操作）
        for (StepperMotor *motor : motorsToStep)
        {
            motor->updatePosition();
        }
    }

    // 5. 位置报告逻辑
    if (HAL_GetTick() - _lastPositionUpdate >= M114_CACHE_INTERVAL)
    {
        // 构建位置字符串
        char temp[100];
        _cachedPosition[0] = '\0'; // 清空缓存

        static char PositionScreen[64]; // 4行x16字符
        PositionScreen[0] = '\0';

        size_t remaining = sizeof(PositionScreen) - 1; // 剩余空间，留出结束符位置

        // 安全拼接所有轴的位置信息
        for (char axis : axisLetters)
        {
            // 构建轴位置信息
            snprintf(temp, sizeof(temp), "%c:%.2f ", axis, (double)motorMap[axis]->getCurrentPosition());
            strncat(_cachedPosition, temp, sizeof(_cachedPosition) - 1); // 安全拼接缓存

            // 构建显示信息（带溢出保护）
            snprintf(temp, sizeof(temp), "%c%.1f ", axis, (double)motorMap[axis]->getCurrentPosition());
            size_t len = strlen(temp);

            // 如果剩余空间足够，拼接；否则截断
            if (len <= remaining)
            {
                strcat(PositionScreen, temp);
                remaining -= len;
            }
            else
            {
                // 空间不足时，复制剩余空间能容纳的内容并终止循环
                strncat(PositionScreen, temp, remaining);
                remaining = 0;
                break;
            }
        }

        // 用空格填充剩余位置（如果有）
        if (remaining > 0)
        {
            memset(PositionScreen + strlen(PositionScreen), ' ', remaining);
            PositionScreen[sizeof(PositionScreen) - 1] = '\0'; // 确保结束符
        }

        // 更新显示
        OLED_ShowString(0, 0, PositionScreen);
        _lastPositionUpdate = HAL_GetTick();
    }

    // 6. 归零状态机
    if (_isHoming)
    {
        // 简化的归零状态机
        bool allHomed = true;
        for (char axis : axisLetters)
        {
            if (_homeRequested[axis] && motorMap[axis]->isMoving())
            {
                allHomed = false;
                break;
            }
        }

        if (allHomed)
        {
            // 所有请求归零的轴已完成
            for (char axis : axisLetters)
            {
                if (_homeRequested[axis])
                {
                    motorMap[axis]->setCurrentPosition(0);
                    _homeRequested[axis] = false;
                }
            }
            _isHoming = false;
            printf("Homing complete\r\n");
        }
    }

    // 定义位置比较精度
    const float POSITION_TOLERANCE = 0.001f;

    // 7. 电机自动禁用逻辑
    for (char axis : axisLetters)
    {
        StepperMotor *motor = motorMap[axis];

        // 使用精度比较方法比较浮点数大小
        if (!motor->isMoving() &&
            fabsf(motor->getCurrentPosition() - motor->getTargetPosition()) <= POSITION_TOLERANCE)
        {
            motor->disable();
        }
    }
}

// 处理接收到的命令
void MotionController::processCommand(const char *command)
{
    if (strcmp(command, "help") == 0)
    {
        printHelp();
        return;
    }

    // 检查 G 代码
    else if (strncmp(command, "G", 1) == 0)
    {
        parseGCode(command);
    }
    // 检查 M 代码
    else if (strncmp(command, "M", 1) == 0)
    {
        parseMCode(command);
    }
    else
    {
        printf("未知命令: %s\r\n", command); // 关键：反馈未知命令
    }
}

// 打印帮助信息
void MotionController::printHelp()
{
    printf("\nAvailable commands:\r\n");
    printf("G90 - Set absolute positioning\r\n");
    printf("G91 - Set relative positioning\r\n");
    printf("G28 - Home all axes\r\n");
    printf("G28 [axis] - Home specific axis (e.g., G28 A C L)\r\n");
    printf("G1 [axis][pos] F[speed] - Linear move (e.g., G1 A10.5 B5.2 L20 F15)\r\n");
    printf("M114 - Get current position\r\n");
    printf("M105/M119 - Get endstop status\r\n");
    printf("M84 - Disable all motors\r\n");
    printf("M84 [axis] - Disable specific axis\r\n");
    printf("\nUsage examples:\r\n");
    printf("G28          ; Home all axes\r\n");
    printf("G28 A C      ; Home only A and C axes\r\n");
    printf("G1 A100 F10 ; Move A axis to 100mm at 10mm/s\r\n");
    printf("G91          ; Set relative positioning mode\r\n");
    printf("G1 B10       ; Move B axis +10mm from current position\r\n");
    printf("\nSupported axes: ");
    for (char axis : axisLetters)
    {
        printf("%c ", axis);
    }
    printf("\r\n");
}

// 解析G代码命令
void MotionController::parseGCode(const char *command)
{
    printf("%s\r\n", command);
    // float test = -1.0f;
    // printf("Float test: %.2f\r\n", test); // 应输出-1.00

    // 解析G28归零命令
    if (strncmp(command, "G28", 3) == 0)
    {
        printf("Executing G28 command\r\n");

        // 检查是否有指定轴
        std::map<char, bool> axisFlags;
        float homingSpeed = 80.0f; // 默认归零速度

        // 初始化所有轴标志为false
        for (char axis : axisLetters)
        {
            axisFlags[axis] = false;
        }

        // 检查F参数
        const char *fPtr = strstr(command, "F");
        if (fPtr != nullptr)
        {
            homingSpeed = atof(fPtr + 1);
        }

        // 检查具体要归零的轴
        if (strlen(command) == 3)
        { // 只有G28，归零所有轴
            for (auto &pair : axisFlags)
            {
                pair.second = true;
            }
        }
        else
        {
            // 检查命令中包含哪些轴字母
            const char *ptr = command + 3; // 跳过"G28"
            while (*ptr)
            {
                if (*ptr == ' ')
                {
                    ptr++;
                    continue;
                }

                for (char axis : axisLetters)
                {
                    if (*ptr == axis)
                    {
                        axisFlags[axis] = true;
                        break;
                    }
                }
                ptr++;
            }
        }

        // 执行归零操作
        home(axisFlags, homingSpeed);
    }
    // 绝对坐标模式
    else if (strncmp(command, "G90", 3) == 0)
    {
        setAbsoluteMode(true);
        printf("ok--%s\r\n", command);
    }
    // 相对坐标模式
    else if (strncmp(command, "G91", 3) == 0)
    {
        setAbsoluteMode(false);
        printf("ok--%s\r\n", command);
    }
    // 直线运动命令
    else if (strncmp(command, "G1", 2) == 0)
    {
        std::map<char, float> axisValues; // abc...全部设为-1 ???
        float f = -1;
        const char *ptr = command;

        // 解析所有参数
        while (*ptr)
        {
            // 检查轴参数
            for (char axis : axisLetters)
            {
                if (*ptr == axis)
                {
                    axisValues[axis] = atof(ptr + 1);
                }
            }

            // 检查速度参数
            if (*ptr == 'F')
            {
                f = atof(ptr + 1);
            }

            ptr++;
        }

        if (f <= 0)
        {
            printf("错误:速度必须大于0\n");
            return;
        }
        else
        {
            setFeedrate(f); // 更新全局速度
        }

        // G90模式下检查负值
        if (_absoluteMode)
        {
            for (auto &pair : axisValues)
            {
                if (pair.second < 0)
                {
                    printf("error: Negative value not allowed in G90 mode for axis %c\r\n", pair.first);
                    return;
                }
            }
        }

        // 运动控制
        bool anyAxisMoved = false;
        for (auto &pair : axisValues)
        {
            char axis = pair.first;
            float value = pair.second;
            StepperMotor *motor = motorMap[axis];

            float target = _absoluteMode ? value : motor->getCurrentPosition() + value;

            motor->enable(); // 包含 _isEnabled = true;
            motor->setFeedrate(_globalFeedrate);
            motor->setTargetPosition(target); // 包含 _isMoving = true;
            anyAxisMoved = true;
        }

        if (anyAxisMoved)
        {
            printf("ok--%s\r\n", command);
        }
        else
        {
            printf("error: No valid axis specified\r\n");
        }
    }
    else
    {
        printf("未知命令: %s\r\n", command); // 关键：反馈未知命令
    }
}

// 解析M代码命令
void MotionController::parseMCode(const char *command)
{
    // 位置查询命令
    if (strncmp(command, "M114", 4) == 0)
    {
        reportPosition();
    }
    // 状态查询命令
    else if (strncmp(command, "M105", 4) == 0 || strncmp(command, "M119", 4) == 0)
    {
        reportStatus(); // 这里多了一个M119
    }
    // 电机禁用命令
    else if (strncmp(command, "M84", 3) == 0)
    {
        // 禁用所有电机
        if (strlen(command) == 3)
        {
            for (char axis : axisLetters)
            {
                motorMap[axis]->disable();
            }
        }
        else
        {
            // 禁用指定轴 =========字符串处理有些不一样
            const char *ptr = command + 3; // 跳过"M84"
            while (*ptr)
            {
                if (*ptr == ' ')
                    ptr++; // 跳过空格

                for (char axis : axisLetters)
                {
                    if (*ptr == axis)
                    {
                        motorMap[axis]->disable();
                        break;
                    }
                }
                ptr++;
            }
        }
        printf("ok--%s\r\n", command);
    }
    else
    {
        printf("未知命令: %s\r\n", command); // 关键：反馈未知命令
    }
}

// 检查限位开关状态  ======== 加了按键消抖
void MotionController::checkEndstops()
{
    // 系统未完成初始化时跳过限位检查
    if (!_initialized)
    {
        if (HAL_GetTick() - _initStartTime > INIT_DELAY)
        {
            _initialized = true;
            powerOnSelfTest(); // 执行上电自检
            printf("System initialization complete\n");
        }
        return;
    }

    auto getLimitSwitch = [](char axis) -> std::pair<GPIO_TypeDef *, uint16_t>
    {
        switch (axis)
        {
        case 'A':
            return {LIM1_GPIO_Port, LIM1_Pin};
        case 'B':
            return {LIM2_GPIO_Port, LIM2_Pin};
        case 'C':
            return {LIM3_GPIO_Port, LIM3_Pin};
        case 'D':
            return {LIM4_GPIO_Port, LIM4_Pin};
        case 'E':
            return {LIM5_GPIO_Port, LIM5_Pin};
        case 'L':
            return {LIM6_GPIO_Port, LIM6_Pin};
        case 'M':
            return {LIM7_GPIO_Port, LIM7_Pin};
        case 'N':
            return {LIM8_GPIO_Port, LIM8_Pin};
        case 'O':
            return {LIM9_GPIO_Port, LIM9_Pin};
        case 'P':
            return {LIM10_GPIO_Port, LIM10_Pin};
        default:
            return {nullptr, 0};
        }
    };

    // 检查所有轴的限位开关
    for (char axis : axisLetters)
    {
        StepperMotor *motor = motorMap[axis];
        if (!motor)
            continue;

        // 获取负限位开关引脚
        auto limitPin = getLimitSwitch(axis);
        if (limitPin.first == nullptr)
            continue;

        // 读取当前限位状态
        bool currentState = (HAL_GPIO_ReadPin(limitPin.first, limitPin.second) == GPIO_PIN_RESET);

        // 初始化状态缓存（首次调用时）
        if (_lastLimitState.find(axis) == _lastLimitState.end())
        {
            _lastLimitState[axis] = currentState;
            _debouncedLimitState[axis] = currentState;
            _limitChangeTime[axis] = HAL_GetTick();
        }

        // 消抖逻辑
        if (currentState != _lastLimitState[axis])
        {
            // 状态发生变化，记录时间
            _lastLimitState[axis] = currentState;
            _limitChangeTime[axis] = HAL_GetTick();
        }
        else if ((HAL_GetTick() - _limitChangeTime[axis]) > LIMIT_DEBOUNCE_TIME)
        {
            // 状态保持稳定超过消抖时间，更新为有效状态
            if (_debouncedLimitState[axis] != currentState)
            {
                _debouncedLimitState[axis] = currentState;

                // 状态确认变化，执行相应操作
                if (_debouncedLimitState[axis])
                {
                    // 限位开关触发
                    printf("Axis %c limit switch triggered\n", axis);

                    // 归零状态处理
                    if (_isHoming && _homeRequested[axis])
                    {
                        motor->stop();
                        motor->setCurrentPosition(0);
                        _homeRequested[axis] = false;
                    }
                    // 非归零状态下的常规运动
                    else if (motor->isMoving())
                    {
                        float movement = motor->getTargetPosition() - motor->getCurrentPosition();
                        if (movement < 0)
                        {
                            motor->stop();
                            printf("Emergency stop: Axis %c limit triggered!\n", axis);
                        }
                    }
                }
                else
                {
                    // 限位开关释放
                    printf("Axis %c limit switch released\n", axis);
                }
            }
        }
    }
}

// 执行归零操作  ============= 没有考虑先后顺序流程 每个轴的归零指令分开
void MotionController::home(const std::map<char, bool> &axisFlags, float homingSpeed)
{
    _isHoming = true;

    // 设置归零速度
    setFeedrate(homingSpeed);

    // 设置归零请求标志
    for (auto &pair : axisFlags)
    {
        _homeRequested[pair.first] = pair.second;
    }

    // 启动归零运动
    for (auto &pair : axisFlags)
    {
        if (pair.second)
        { // 该轴需要归零
            StepperMotor *motor = motorMap[pair.first];
            _homeRequested[pair.first] = true;
            motor->enable();
            motor->setFeedrate(homingSpeed);
            // 向负方向移动直到触发限位
            float moveDistance = motor->getMaxPosition();
            motor->setTargetPosition(motor->getCurrentPosition() - moveDistance);
        }
    }

    printf("Homing started for axes: ");
    for (auto &pair : axisFlags)
    {
        if (pair.second)
            printf("%c ", pair.first);
    }
    printf("\r\n");
}

// 移动到指定坐标
void MotionController::moveTo(const std::map<char, float> &positions)
{
    if (_isEmergencyStop)
        return;

    // 遍历所有指定的轴
    for (const auto &axisPos : positions)
    {
        char axis = axisPos.first;
        float pos = axisPos.second;

        // 检查该轴是否在电机映射中
        if (motorMap.find(axis) != motorMap.end())
        {
            StepperMotor *motor = motorMap[axis];
            motor->enable();
            motor->setFeedrate(_globalFeedrate);
            motor->setTargetPosition(pos);
        }
    }
}

// 获取所有轴当前位置
std::map<char, float> MotionController::getPositions() const
{
    std::map<char, float> positions;
    for (char axis : axisLetters)
    {
        positions[axis] = motorMap.at(axis)->getCurrentPosition();
    }
    return positions;
}

// 停止所有运动
void MotionController::stop()
{
    for (char axis : axisLetters)
    {
        motorMap[axis]->stop();
    }
}

// 紧急停止
void MotionController::emergencyStop()
{
    _isEmergencyStop = true;
    stop();
}

// 报告当前位置
void MotionController::reportPosition()
{
    // 使用缓存的位置信息
    if (strlen(_cachedPosition) > 0)
    {
        printf("%s\r\n", _cachedPosition);
    }
    else
    {
        for (char axis : axisLetters)
        {
            printf("%c:%.2f ", axis, (double)motorMap[axis]->getCurrentPosition());
        }
        printf("\r\n");
    }
    printf("实际速度: ");
    for (char axis : axisLetters)
    {
        printf("%c:%.1fmm/s ", axis, motorMap[axis]->getCurrentFeedrate());
    }
    printf("\n");
}

// 报告系统状态
void MotionController::reportStatus()
{
    if (_isEmergencyStop)
    {
        printf("Status: EMERGENCY_STOP\r\n");
    }
    else if (_isHoming)
    {
        printf("Status: HOMING\r\n");
    }
    else
    {
        bool moving = false;
        for (char axis : axisLetters)
        {
            if (motorMap[axis]->isMoving())
            {
                moving = true;
                break;
            }
        }
        printf("Status: %s\r\n", moving ? "MOVING" : "IDLE");
    }
}

// 设置运动速度 =====从每个轴的速度改为全局速度了 （把所有轴的速度都设成了全局速度）
void MotionController::setFeedrate(float feedrate)
{
    if (feedrate > 0)
    {
        _globalFeedrate = feedrate;
    }
}

// 设置加速度
void MotionController::setAcceleration(float acceleration)
{
    if (acceleration > 0)
    {
        _currentAcceleration = acceleration;
        for (char axis : axisLetters)
        {
            motorMap[axis]->setAcceleration(acceleration);
        }
    }
}

// 获取当前进给速度 ====改为全局速度了 ???
float MotionController::getCurrentFeedrate() const
{
    return _globalFeedrate;
}

// 获取当前加速度
float MotionController::getCurrentAcceleration() const
{
    return _currentAcceleration;
}

// 设置坐标模式
void MotionController::setAbsoluteMode(bool absolute)
{
    _absoluteMode = absolute;
}

// 获取当前坐标模式
bool MotionController::isAbsoluteMode() const
{
    return _absoluteMode;
}